#ifndef __UTILITY_FUNCTIONS__
#define __UTILITY_FUNCTIONS__

#include <iostream>

template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());

  index j;
  index m = (p.last_leaf() >> 10) << 5;
  while (i < m) {
    const position ak = a + i;
    *(ak) = *(ak + 1);
    j = less(*(ak + 2), *(ak + 3)) ? (i | 3) : (i | 2);
    *(ak + 1) = *(a + j);
    index k = (j & 15) << 1;
    index k2 = k + 1;
    k = less(*(ak + k), *(ak + k2)) ? (i | k2) : (i | k);       
    *(a + j) = *(a + k);
    j = (k & 31) << 1;
    k2 = j + 1;
    j = less(*(ak + j), *(ak + k2)) ? (i | k2) : (i | j);    
    *(a + k) = *(a + j);
    k = (j & 31) << 1;
    k2 = k + 1;
    k = less(*(ak + k), *(ak + k2)) ? (i | k2) : (i | k);    
    *(a + j) = *(a + k);
    i = (k << 6) - ((k >> 5) << 10) - 960; 
    position ak2 = a + i;
    if(less(*(ak2), *(ak2 - 32))) i -= 32;
    if(less(*(a + i), x)) {
      index k1 = (k & 31); 
      if(less(*(a + k), x)) {
	*(a + j) = *(ak + (k1 >> 2));
	if(less(*(a + j), x)) {
	  *(ak + (k1 >> 2)) = *(ak + (k1 >> 3));
	    if(less(*(ak + (k1 >> 2)), x)) {
	      *(ak + (k1 >> 3)) = *(ak + 1);	      
	      if(less(*(ak + (k1 >> 3)), x)) {   
		*(ak + 1) = *(ak); 
		if(less(*(ak), x))	
		  *(ak) = x;
		else
		  *(ak + 1) = x;	
	      }
	      else
		*(ak + (k1 >> 3)) = x;
	    }
	    else
	      *(ak + (k1 >> 2)) = x;
	}
	else 
	  *(a + j) = x;    
      }
      else 
	*(a + k) = x;       
	return;
    }
    else
      *(a + k) =  *(a + i);      	 
  }  
  j = i | 16;
  if ((((j << 6) - (i << 5) - 992) > p.last_leaf()) && ((i | 31) <= p.last_leaf())) { 
    const position ak = a + i;
    if(less(*(ak + 1), x)) {
      *(ak) = x;
      return;
    }
    j = less(*(ak + 2), *(ak + 3)) ? (i | 3) : (i | 2);
    *(ak) = *(ak + 1);
    if(less(*(a + j), x)) {
      *(ak + 1) = x;
      return;
    }
    index k = (j & 15) << 1;
    k = less(*(ak + k), *(ak + k + 1)) ? (i | k + 1) : (i | k);
    *(ak + 1) = *(a + j);
    if(less(*(a + k), x)) {
      *(a + j) = x;
      return;
    }
    *(a + j) = *(a + k);
    j = (k & 15) << 1;
    j = less(*(ak + j), *(ak + j + 1)) ? (i | j + 1) : (i | j);
    if(less(*(a + j), x)) {
      *(a + k) = x;
      return;
    }
    *(a + k) = *(a + j);
    k = (j & 15) << 1;
    k = less(*(ak + k), *(ak + k + 1)) ? (i | k + 1) : (i | k);
    if(less(*(a + k), x)) {
      *(a + j) = x;
      return;
    }
    *(a + j) = *(a + k);
    *(a + k) = x;
    return;
  }
  while (p.last_child(i) <= p.last_leaf()) {
    j = p.top_all_present(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
    i = j;
  }
  while(p.first_child(i) <= p.last_leaf()) {
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i, less);	
    else 
      j = p.top_some_absent(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
      i = j;
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
  }    
  p.update_all_present(a, i, x, less);
  return;      

  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

}

template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down2(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());
  
  index j;
  while (p.last_child(i) <= p.last_leaf()) {
    j = p.top_all_present(a, i,  less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
    i = j;
  }
  
  while(p.first_child(i) <= p.last_leaf()) {
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i, less);	
    else 
      j = p.top_some_absent(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
      i = j;
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
  }    
  p.update_all_present(a, i, x, less);
  return; 

  
  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

} 

#endif // __UTILITY_FUNCTIONS__


